/******************************************************************************\
 *                                                                            *
 *                               Math stuff                                   *
 *                   Flavio Bertini fl.bertini@gmail.com                      *
 *                                                                            *
 *                                                                            *
\******************************************************************************/


#include "Math.h"

// MATH STUFF
// check if two floats are almost equal
// http://www.cygnus-software.com/papers/comparingfloats/comparingfloats.htm
bool mat_almostEqual(float a, float b, int maxUlps)
{
  // make sure maxUlps is non-negative and small enough that
  // the default NAN won't compare as equal to anything.
  assert(maxUlps > 0 && maxUlps < 4 * 1024 * 1024);
  int aInt = *(int*)&a;
  // make aInt lexicographically ordered as a twos-complement int
  if (aInt < 0)
    aInt = 0x80000000 - aInt;
  // Make bInt lexicographically ordered as a twos-complement int
  int bInt = *(int*)&b;
  if (bInt < 0)
    bInt = 0x80000000 - bInt;
  int intDiff = abs(aInt - bInt);
  if (intDiff <= maxUlps)
    return true;
  return false;
}


// creates a new empty Mat_Vector
Mat_Vector::Mat_Vector()
{
  x = 0.0;
  y = 0.0;
  z = 0.0;
  return;
}

// creates a new Mat_Vector starting from a Vector
Mat_Vector::Mat_Vector(const Vector vector)
{
  x = vector.x;
  y = vector.y;
  z = vector.z;
  return;
}

// creates a new Vector with the specified component values
Mat_Vector::Mat_Vector(float _x, float _y, float _z)
{
  x = _x;
  y = _y;
  z = _z;
  return;
}

// set zero all coordinates
void Mat_Vector::mat_setZero(void)
{
  x = 0.0;  y = 0.0;  z = 0.0;
  return;
}

// transform cartesian coordinates to spherical coordinates
void Mat_Vector::mat_cartesian2sphere(void)
{
  rho   = this->magnitude();
  if (!mat_almostEqual(rho, 0.0, 2))
  {
    // [0÷360 radians from the z axis]
    if (!mat_almostEqual(z, 0.0, 2))
    {
      phi = atan(x/z);
      if ((x>0.0 && z<0.0) || (x<0.0 && z<0.0))
        phi = PI + phi;
      else if (x<0.0 && z>0.0)
        phi = 2.0*PI + phi;
    }
    else
    {
      phi = (x>0.0) ? (PI/2.0) : (2.0*PI-(PI/2.0));
    }
    theta = acos(y/rho);
  }
  else
  {
    phi = theta = 0.0;
  }
  return;
}

// transform spherical coordinates to cartesian coordinates
void Mat_Vector::mat_sphere2cartesian(void)
{
  short count = 0;                // how many sign inversion preserving the position
  short countPos[] = {0, 0, 0};
  float tx = rho*sin(theta)*sin(phi);
  float ty = rho*cos(theta);
  float tz = rho*sin(theta)*cos(phi);
  // to comply the orientation
  if ((tx>0 && x<0) || (tx<0 && x>0)) { count +=1;  countPos[0] = 1; }
  if ((ty>0 && y<0) || (ty<0 && y>0)) { count +=1;  countPos[1] = 1; }
  if ((tz>0 && z<0) || (tz<0 && z>0)) { count +=1;  countPos[2] = 1; }
  if (count==0 || count == 3)
  {
    x = tx;  y = ty;  z = tz;
  }
  else
  {
    if (countPos[0]==1) { x = -1 * tx; } else { x = tx; }
    if (countPos[1]==1) { y = -1 * ty; } else { y = ty; }
    if (countPos[2]==1) { z = -1 * tz; } else { z = tz; }
  }
  return;
}

// translate the point A to the point B by translation transl
void mat_translateVector(Mat_Vector pointA, Mat_Vector transl, Mat_Vector &pointB)
{
  float temp[3];
  temp[0] = pointA.x + transl[0];
  temp[1] = pointA.y + transl[1];
  temp[2] = pointA.z + transl[2];
  pointB.x = temp[0];
  pointB.y = temp[1];
  pointB.z = temp[2];
  return;
}

void mat_translateVector(float pointA[3], Mat_Vector transl, float pointB[3])
{
  float temp[3];
  temp[0] = pointA[0] + transl[0];
  temp[1] = pointA[1] + transl[1];
  temp[2] = pointA[2] + transl[2];
  pointB[0] = temp[0];
  pointB[1] = temp[1];
  pointB[2] = temp[2];
  return;
}

// multiply vector-matrix
void mat_vectorMatrixMultiply(float pointA[3], float M[16], float pointB[3])
{
  float temp[3];
  temp[0] = pointA[0]*M[0] + pointA[1]*M[4] + pointA[2]*M[8];
  temp[1] = pointA[0]*M[1] + pointA[1]*M[5] + pointA[2]*M[9];
  temp[2] = pointA[0]*M[2] + pointA[1]*M[6] + pointA[2]*M[10];
  pointB[0] = temp[0];
  pointB[1] = temp[1];
  pointB[2] = temp[2];
  return;
}

// compute normal between three points
Vector mat_computeNormal(vector<Vector> triangle)
{
  Vector v1, v2, normal;
  v1.x = triangle[0].x - triangle[1].x;
  v1.y = triangle[0].y - triangle[1].y;
  v1.z = triangle[0].z - triangle[1].z;
  v2.x = triangle[2].x - triangle[1].x;
  v2.y = triangle[2].y - triangle[1].y;
  v2.z = triangle[2].z - triangle[1].z;
  normal.x = v1.y * v2.z - v1.z * v2.y;
  normal.y = v1.z * v2.x - v1.x * v2.z;
  normal.z = v1.x * v2.y - v1.y * v2.x;
  return normal;
}

// compute normal between three points array
Vector mat_computeNormal(float triangle[9])
{
  Vector v1, v2, normal;
  v1.x = triangle[0] - triangle[3];
  v1.y = triangle[1] - triangle[4];
  v1.z = triangle[2] - triangle[5];
  v2.x = triangle[6] - triangle[3];
  v2.y = triangle[7] - triangle[4];
  v2.z = triangle[8] - triangle[5];
  normal.x = v1.y * v2.z - v1.z * v2.y;
  normal.y = v1.z * v2.x - v1.x * v2.z;
  normal.z = v1.x * v2.y - v1.y * v2.x;
  return normal;
}

// load identity
void mat_glMatrixfIdentity(float M[16])
{
  for (int i=0; i<4; i++)
    for (int j=0; j<4; j++)
      M[i*4+j] = (i==j);
  return;
}

// multiply matrix
void mat_glMatrixfMultiply(float A[16], float B[16], float C[16])
{
  float res[16];

  for(int i=0; i<4; i++)
  {
    for(int j=0; j<4; j++)
    {
      res[i*4+j] = 0.0;
      for(int k=0; k<4; k++)
      {
        res[i*4+j] += A[i*4+k] * B[k*4+j];
      }
    }
  }

  for (int i=0; i<16; i++)
    C[i]=res[i];

  return;
}

// invert matrix
int mat_glMatrixInvert(float src[16], float dst[16])
{
  float t;
  float tmp[4][4];

  mat_glMatrixfIdentity(dst);
  for (int i=0; i<4; i++)
  {
    for (int j=0; j<4; j++)
    {
      tmp[i][j] = src[i*4+j];
    }
  }

  for (int i=0; i<4; i++)
  {
    // look for largest element in column
    int swap = i;
    for (int j=i+1; j<4; j++)
    {
      if (fabs(tmp[j][i]) > fabs(tmp[i][i]))
      {
        swap = j;
      }
    }

    if (swap != i)
    {
      // swap rows
      for (int k=0; k<4; k++)
      {
        t = tmp[i][k];
        tmp[i][k] = tmp[swap][k];
        tmp[swap][k] = t;

        t = dst[i*4+k];
        dst[i*4+k] = dst[swap*4+k];
        dst[swap*4+k] = t;
      }
    }

    // no non-zero pivot: the matrix is singular, which shouldn't happen
    if (tmp[i][i] == 0)
    {
      return 0;
    }

    t = tmp[i][i];
    for (int k=0; k<4; k++)
    {
      tmp[i][k] /= t;
      dst[i*4+k] /= t;
    }
    for (int j=0; j<4; j++)
    {
      if (j != i)
      {
        t = tmp[j][i];
        for (int k=0; k<4; k++)
        {
          tmp[j][k] -= tmp[i][k]*t;
          dst[j*4+k] -= dst[i*4+k]*t;
        }
      }
    }
  }

  return 1;
}

// trackball theory to compute the rotation matrix
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
void mat_computeRotationMatrix(Mat_Vector v1, Mat_Vector v2, float M[16])
{
  // compute the angle between the vectors: acos(dotProduct/(lengthV1*lengthV2))
  float num = v1.dot(v2);
  float den = (v1.magnitude()*v2.magnitude());
  float alpha = 0.0;
  if ( !mat_almostEqual(num, den, 2) )
    alpha = acos(v1.dot(v2)/(v1.magnitude()*v2.magnitude()));

  // compute the normalized cross product
  Mat_Vector cProduct = v2.cross(v1).normalized();

  // compute the rotation matrix
  float cosa = cos(alpha);
  float sina = sin(alpha);
  float t = 1 - cosa;
  float x = cProduct[0];
  float y = cProduct[1];
  float z = cProduct[2];
  M[0]  = t*x*x + cosa;
  M[1]  = t*x*y - z*sina;
  M[2]  = t*x*z + y*sina;
  M[3]  = 0.0;
  M[4]  = t*x*y + z*sina;
  M[5]  = t*y*y + cosa;
  M[6]  = t*y*z - x*sina;
  M[7]  = 0.0;
  M[8]  = t*x*z - y*sina;
  M[9]  = t*y*z + x*sina;
  M[10] = t*z*z + cosa;
  M[11] = 0.0;
  M[12] = 0.0;
  M[13] = 0.0;
  M[14] = 0.0;
  M[15] = 1.0;

  return;
}

// compute the rotation matrix
void mat_computeRotationMatrixFix(Vector v, float alpha, float M[16])
{
  // compute the rotation matrix
  float cosa = cos(alpha);
  float sina = sin(alpha);
  float t = 1 - cosa;
  M[0]  = t*v.x*v.x + cosa;
  M[1]  = t*v.x*v.y - v.z*sina;
  M[2]  = t*v.x*v.z + v.y*sina;
  M[3]  = 0.0;
  M[4]  = t*v.x*v.y + v.z*sina;
  M[5]  = t*v.y*v.y + cosa;
  M[6]  = t*v.y*v.z - v.x*sina;
  M[7]  = 0.0;
  M[8]  = t*v.x*v.z - v.y*sina;
  M[9]  = t*v.y*v.z + v.x*sina;
  M[10] = t*v.z*v.z + cosa;
  M[11] = 0.0;
  M[12] = 0.0;
  M[13] = 0.0;
  M[14] = 0.0;
  M[15] = 1.0;

  return;
}

// rotation defined around coordinate axes
void mat_glMatrixXrot(float angle, float M[16])
{
  M[0]  = 1.0;
  M[1]  = 0.0;
  M[2]  = 0.0;
  M[3]  = 0.0;
  M[4]  = 0.0;
  M[5]  = cos(angle);
  M[6]  = -sin(angle);
  M[7]  = 0.0;
  M[8]  = 0.0;
  M[9]  = sin(angle);
  M[10] = cos(angle);
  M[11] = 0.0;
  M[12] = 0.0;
  M[13] = 0.0;
  M[14] = 0.0;
  M[15] = 1.0;
  return;
}

void mat_glMatrixYrot(float angle, float M[16])
{
  M[0]  = cos(angle);
  M[1]  = 0.0;
  M[2]  = sin(angle);
  M[3]  = 0.0;
  M[4]  = 0.0;
  M[5]  = 1.0;
  M[6]  = 0.0;
  M[7]  = 0.0;
  M[8]  = -sin(angle);
  M[9]  = 0.0;
  M[10] = cos(angle);
  M[11] = 0.0;
  M[12] = 0.0;
  M[13] = 0.0;
  M[14] = 0.0;
  M[15] = 1.0;
  return;
}

void mat_glMatrixZrot(float angle, float M[16])
{
  M[0]  = cos(angle);
  M[1]  = -sin(angle);
  M[2]  = 0.0;
  M[3]  = 0.0;
  M[4]  = sin(angle);
  M[5]  = cos(angle);
  M[6]  = 0.0;
  M[7]  = 0.0;
  M[8]  = 0.0;
  M[9]  = 0.0;
  M[10] = 1.0;
  M[11] = 0.0;
  M[12] = 0.0;
  M[13] = 0.0;
  M[14] = 0.0;
  M[15] = 1.0;
  return;
}

// rotate the vector around y axis
void mat_glRotateXZ(float a, Vector *v)
{
  float x = v->x;
  float z = v->z;
  v->x = x*cos(a) - z*sin(a);
  v->z = x*sin(a) + z*cos(a);
  return;
}

/* Effettua l'intersezione del triangolo di vertici v1, v2, v3 con il
   segmento di estremi est1, est2. Restituisce in trian ed in seg
   rispettivamente le coordinate del punto di intersezione trovato nel
   triangolo e nel segmento. fd sara' uguale a true se e' stata trovata una
   intersezione, a false altrimenti. */
void mat_intersection(Vector *v1, Vector *v2, Vector *v3, Vector *est1, Vector *est2,
						Vector *trian, Vector *seg, bool *fd) {

  Vector inter,aux1;
  bool flag;
  double a1,a2,a3,a4,
        b1,b2,b3,b4,
        c1,c2,c3,c4,
        a,b,c,d,
        //l1,l2,l3,
        val1,val2,
        t,d4,num,den,
        alfa,beta,gamma,d1;
  int k1,k2,k3,
      m1,m2,n1,n2,
      p1,p2,g1,g2,g3;

  flag = true;
  *fd = false;

  /* Testa se il segmento appartiene interamente ad uno dei due semispazi
     divisi dal piano del triangolo: ax + by + cz + d = 0 */
  a = (v2->y - v1->y)*(v3->z - v1->z)-(v3->y - v1->y)*(v2->z - v1->z);
  b = (v3->x - v1->x)*(v2->z - v1->z)-(v2->x - v1->x)*(v3->z - v1->z);
  c = (v2->x - v1->x)*(v3->y - v1->y)-(v3->x - v1->x)*(v2->y - v1->y);
  d = (-a)*v1->x-b*v1->y-c*v1->z;

  /* sostituendo i due estremi del segmento nell'equazione precedente. */
  val1 = a*est1->x + b*est1->y + c*est1->z + d;
  val2 = a*est2->x + b*est2->y + c*est2->z + d;

  if ( !((val1 > 0 && val2 > 0) || (val1 < 0 && val2 < 0)) ) {
    /* Il segmento non appartiene interamente ad uno dei due semispazi
       divisi dal piano del triangolo, quindi i due oggetti potrebbero
       intersecarsi */

    mat_perp(v1,v2,a,b,c,&a1,&b1,&c1);
    mat_perp(v1,v3,a,b,c,&a2,&b2,&c2);
    mat_perp(v2,v3,a,b,c,&a3,&b3,&c3);

    mat_piano(v3,v1,a1,b1,c1,&k1);
    mat_piano(v2,v1,a2,b2,c2,&k2);
    mat_piano(v1,v2,a3,b3,c3,&k3);

    mat_piano(est1,v1,a1,b1,c1,&m1);
    mat_piano(est2,v1,a1,b1,c1,&m2);

    mat_piano(est1,v1,a2,b2,c2,&n1);
    mat_piano(est2,v1,a2,b2,c2,&n2);

    mat_piano(est1,v2,a3,b3,c3,&p1);
    mat_piano(est2,v2,a3,b3,c3,&p2);

    if ( !((m1 == m2 && m1 == -k1) || (n1 == n2 && n2 == -k2) ||
          (p1 == p2 && p1 == -k3)) ) {

      if ( (d1=DIST_3D((*est1), (*est2))) != 0 ) {
        alfa = (est2->x - est1->x) / d1;
        beta = (est2->y - est1->y) / d1;
        gamma = (est2->z - est1->z) / d1;
        num = a*est1->x + b*est1->y + c*est1->z + d;
        den = a*alfa + b*beta + c*gamma;
      }

      if (den != 0) {
        t = num/den;
        inter.x = est1->x - t*alfa;
        inter.y = est1->y - t*beta;
        inter.z = est1->z - t*gamma;
        mat_piano(&inter,v1,a1,b1,c1,&g1);
        mat_piano(&inter,v1,a2,b2,c2,&g2);
        mat_piano(&inter,v2,a3,b3,c3,&g3);

        /* Si determina se il punto di intersezione è interno, il segno
           della somma del prodotto delle componenti di due vettori, è
           =0 se si trovano a 90°, <1 se si trovano a >90° e >1 se si trovano
           a <90°.
           Se i due vettori sono dati dal punto di intersezione e la normale
           tra i lati e la perpendicolare al piano, si riesce a determinar se
           l'intersezione è interna al troangolo. */
        if ((g1 == 0 || g1 == k1) && (g2 == 0 || g2 == k2) &&
            (g3 == 0 || g3 == k3)) {

          *fd = true;

          if ((v1->x == inter.x) && (v1->y == inter.y) &&
              (v1->z==inter.z)) {
            SWAP(&(v1->x),&(v2->x));
            SWAP(&(v1->y),&(v2->y));
            SWAP(&(v1->z),&(v2->z));
            SWAP(&a2,&a3);
            SWAP(&b2,&b3);
            SWAP(&c2,&c3);
/*
            SWAP(&(v1->par[0]),&(v2->par[0]));
            SWAP(&(v1->par[1]),&(v2->par[1]));
*/
            flag = false;
          }
          mat_perp(v1,&inter,a,b,c,&a4,&b4,&c4);
          d1 = DIST_3D((*v2),(*v3));
          alfa = (v3->x - v2->x)/d1;
          beta = (v3->y - v2->y)/d1;
          gamma = (v3->z - v2->z)/d1;
          d4 = (-a4)*inter.x - b4*inter.y - c4*inter.z;
          num = a4*v2->x + b4*v2->y + c4*v2->z + d4;
          den = a4*alfa + b4*beta + c4*gamma;

          if (den != 0) {
            t = num/den;
            aux1.x = v2->x - t*alfa;
            aux1.y = v2->y - t*beta;
            aux1.z = v2->z - t*gamma;
         /*   l1 = */mat_lambda(v2,v3,&aux1);
/*
            aux1.par[0] = l1*v3->par[0] + (1-l1)*v2->par[0];
            aux1.par[1] = l1*v3->par[1] + (1-l1)*v2->par[1];
*/
            /* l2 = */mat_lambda(v1,&aux1,&inter);
            COPY_3D((*trian), inter);
/*
            trian->par[0] = l2*aux1.par[0] + (1-l2)*v1->par[0];
            trian->par[1] = l2*aux1.par[1] + (1-l2)*v1->par[1];
*/
            /* l3 = */mat_lambda(est1,est2,&inter);
            COPY_3D((*seg), inter);
/*
            seg->par[0] = l3*est2->par[0] + (1-l3)*est1->par[0];
            seg->par[1] = l3*est2->par[1] + (1-l3)*est1->par[1];
*/
            if (!flag) {
              SWAP(&(v1->x),&(v2->x));
              SWAP(&(v1->y),&(v2->y));
              SWAP(&(v1->z),&(v2->z));
/*
              SWAP(&(v1->par[0]),&(v2->par[0]));
              SWAP(&(v1->par[1]),&(v2->par[1]));
*/
            }
          }
        }
      }
    }
  }

  return;
}

/* Perpendicolare tre due vettori */
void mat_perp(Vector *p1, Vector *p2, double a, double b, double c,
                 double *x, double *y, double *z) {

  double p,q,r;

  p = (p2->x) - (p1->x);
  q = (p2->y) - (p1->y);
  r = (p2->z) - (p1->z);
  *x = q*c - r*b;
  *y = r*a - p*c;
  *z = p*b - q*a;

  return;
}

/* Segno del prodotto scalare tra due vettori:
   <0 : >90° ;
   =0 : =90° ;
   >0 : <90° . */
void mat_piano(Vector *vert1, Vector *vert2, double a, double b, double c,
                  int *k) {

  *k = SEGNO((vert1->x - vert2->x)*a + (vert1->y - vert2->y)*b +
             (vert1->z - vert2->z)*c);

  return;
}

double mat_lambda(Vector *vert1, Vector *vert2, Vector *interno) {

  double pp,p,q,r;

  p = fabs(vert1->x - vert2->x);
  q = fabs(vert1->y - vert2->y);
  r = fabs(vert1->z - vert2->z);

  if ((p >= q) && (p >= r))
    pp = (interno->x - vert1->x) / (vert2->x - vert1->x);
  else if ((q >= p) && (q >= r))
    pp = (interno->y - vert1->y) / (vert2->y - vert1->y);
  else
    pp = (interno->z - vert1->z) / (vert2->z - vert1->z);

  return(pp);
}

/* Distanza tra due punti */
double mat_pointDistance(Vector v1,Vector v2){

	return sqrt((v1.x-v2.x)*(v1.x-v2.x)+(v1.y-v2.y)*(v1.y-v2.y)+(v1.z-v2.z)*(v1.z-v2.z));

}


